import numpy as np
import matplotlib.pyplot as plt
from pylab import mpl

# 设置显示中文字体
mpl.rcParams["font.sans-serif"] = ["SimHei"]
# 设置正常显示符号
mpl.rcParams["axes.unicode_minus"] = False
# 参数
a = 55 / (2 * np.pi)  # 螺线的参数


# v_a = 1  # a点的线速度，单位：cm/s
# fixed_distance = 165  # a和b之间的固定欧式距离


# dt = 10  # 时间步长，单位：秒


def calculate_theta_a(x_0, y_0):
    """计算a点的极角"""
    r = np.sqrt(x_0 ** 2 + y_0 ** 2)
    theta_a = r / a
    return theta_a


def calculate_theta_b(x_0, y_0, theta_a, fixed_distance):
    """计算b点的极角"""
    theta_b = theta_a
    while True:
        theta_b += 0.01  # 逐步增加theta_b，直到满足条件
        x_b = a * theta_b * np.cos(theta_b)
        y_b = a * theta_b * np.sin(theta_b)
        if np.sqrt((x_b - x_0) ** 2 + (y_b - y_0) ** 2) >= fixed_distance:
            break
    return theta_b


def calculate_b_coordinates(theta_b):
    """计算b点的坐标"""
    x_b = a * theta_b * np.cos(theta_b)
    y_b = a * theta_b * np.sin(theta_b)
    return x_b, y_b


def calculate_slope(theta):
    up = (np.sin(theta) + theta * np.cos(theta))
    down = (np.cos(theta) - theta * np.sin(theta))
    return np.arctan(up / down)


def calculate_velocity_components(v_a, theta_a, theta_b, x_0, y_0, x_b, y_b):
    """计算速度分解到ab线段方向的分量"""
    # 计算螺线在a点和b点的切线角度
    # tangent_angle_a = theta_a + np.pi / 2
    # tangent_angle_b = theta_b + np.pi / 2

    tangent_angle_a = calculate_slope(theta_a)
    tangent_angle_b = calculate_slope(theta_b)

    # 计算ab线段的角度
    ab_angle = np.arctan2(y_b - y_0, x_b - x_0)

    # 计算速度分解到ab线段方向的分量
    v_a_line = v_a * np.cos(tangent_angle_a - ab_angle)
    v_b_line = v_a_line
    v_b = v_b_line / np.cos(tangent_angle_b - ab_angle)
    return abs(v_b)


def calculate_new_coordinate(pos, v_a, dt):
    theta_a = calculate_theta_a(pos[0], pos[1])
    new_theta_a = theta_a - v_a / a * dt
    new_x_a = a * new_theta_a * np.cos(new_theta_a)
    new_y_a = a * new_theta_a * np.sin(new_theta_a)
    return new_x_a, new_y_a
    # x_0, y_0 = pos
    # t = np.sqrt(x_0**2 + y_0**2) / a  # 估计参数 t
    # dx_dt = a * (np.cos(t) - t * np.sin(t))
    # dy_dt = a * (np.sin(t) + t * np.cos(t))
    # alpha = np.arctan2(dy_dt, dx_dt)  # 计算切线的倾斜角
    # new_x_a = x_0 + v_a * dt * np.cos(alpha)
    # new_y_a = y_0 + v_a * dt * np.sin(alpha)
    # return new_x_a, new_y_a


def calculate_new_coordinates(theta_a, theta_b, v_a, v_b, dt, fixed_distance):
    """计算1秒后a和b的坐标"""
    new_theta_a = theta_a - v_a / a * dt
    new_theta_b = new_theta_a + np.arccos(1 - (fixed_distance ** 2) / (2 * a ** 2 * new_theta_a ** 2))
    new_x_a = a * new_theta_a * np.cos(new_theta_a)
    new_y_a = a * new_theta_a * np.sin(new_theta_a)
    new_x_b = a * new_theta_b * np.cos(new_theta_b)
    new_y_b = a * new_theta_b * np.sin(new_theta_b)
    return new_x_a, new_y_a, new_x_b, new_y_b


def calculate_pos(x_0, y_0, v_0, fixed_distance):
    if x_0 == 0 and y_0 == 0 and v_0 == 0:
        return 0, 0, 0,

    # 计算theta_a
    theta_a = calculate_theta_a(x_0, y_0)

    # 计算theta_b
    theta_b = calculate_theta_b(x_0, y_0, theta_a, fixed_distance)
    # 如果还没进入入口，返回-1
    if theta_b > 16 * 2 * np.pi:
        return 0, 0, 0

    # 计算b点的坐标
    x_b, y_b = calculate_b_coordinates(theta_b)

    # 计算b点的速度
    v_b = calculate_velocity_components(v_0, theta_a, theta_b, x_0, y_0, x_b, y_b)
    return x_b, y_b, v_b


def main():
    v_a = 1
    dt = 10
    fixed_distance = 165
    # 输入a点的坐标
    x_0 = float(input("请输入a点的x坐标(cm): "))
    y_0 = float(input("请输入a点的y坐标(cm): "))

    # 计算theta_a
    theta_a = calculate_theta_a(x_0, y_0)

    # 计算theta_b
    theta_b = calculate_theta_b(x_0, y_0, theta_a, fixed_distance)

    # 计算b点的坐标
    x_b, y_b = calculate_b_coordinates(theta_b)

    # 计算b点的速度
    v_b = calculate_velocity_components(v_a, theta_a, theta_b, x_0, y_0, x_b, y_b)

    # 输出结果
    print(f"b点的坐标为: ({x_b:.2f}, {y_b:.2f}) cm")
    print(f"b点的速度为: {v_b:.8f} cm/s")

    # 计算1秒后a和b的坐标
    new_x_a, new_y_a, new_x_b, new_y_b = calculate_new_coordinates(theta_a, theta_b, v_a, v_b, dt)

    # 输出1秒后a和b的坐标
    print(f"{dt}秒后a点的坐标为: ({new_x_a:.2f}, {new_y_a:.2f}) cm")
    print(f"{dt}秒后b点的坐标为: ({new_x_b:.2f}, {new_y_b:.2f}) cm")

    # 绘制等距螺线
    theta_values = np.linspace(0, 50 * np.pi, 5000)
    x_values = a * theta_values * np.cos(theta_values)
    y_values = a * theta_values * np.sin(theta_values)

    # 绘制初始a点、初始b点、1秒后a点和1秒后b点
    plt.plot(x_values, y_values, 'k--', label='等距螺线')
    plt.plot(x_0, y_0, 'ro', label='初始a点')
    plt.plot(x_b, y_b, 'bo', label='初始b点')
    plt.plot(new_x_a, new_y_a, 'go', label=f'{dt}秒后a点')
    plt.plot(new_x_b, new_y_b, 'mo', label=f'{dt}秒后b点')

    # 添加图例
    plt.legend()

    # 显示图形
    plt.show()


if __name__ == "__main__":
    main()
